#全参数解算

# Step 1: 导入必要的库
import numpy as np
from scipy.spatial.transform import Rotation
import random
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import pandas as pd
import cv2
from scipy.optimize import least_squares
import tkinter as tk
from tkinter import ttk, scrolledtext
import threading
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg

# Step 2: 设置numpy打印选项
np.set_printoptions(suppress=True, precision=6)

# Step 3: 定义相机内参数
# 3.1 图像分辨率
IMAGE_WIDTH = 1280
IMAGE_HEIGHT = 1024

# 3.2 物理参数
FOCAL_LENGTH_MM = 25  # 焦距（毫米）
PIXEL_SIZE_UM = 5.3   # 像元大小（微米）

# 3.3 计算内参
FOCAL_LENGTH_PIXELS = FOCAL_LENGTH_MM * 1000 / PIXEL_SIZE_UM  # 焦距（像素）
PRINCIPAL_POINT_X = IMAGE_WIDTH / 2   # 主点x坐标（像素）
PRINCIPAL_POINT_Y = IMAGE_HEIGHT / 2  # 主点y坐标（像素）

# 3.4 构建内参矩阵
INTRINSIC_MATRIX = np.array([
    [FOCAL_LENGTH_PIXELS, 0, PRINCIPAL_POINT_X],
    [0, FOCAL_LENGTH_PIXELS, PRINCIPAL_POINT_Y],
    [0, 0, 1]
])

# 相机参数
CAMERA_PARAMS = {
    'focal_length': FOCAL_LENGTH_MM,    # 毫米
    'pixel_width': IMAGE_WIDTH,
    'pixel_height': IMAGE_HEIGHT,
    'pixel_size': PIXEL_SIZE_UM         # 微米
}

# Step 4: 定义全站仪外参数
# 4.1 姿态角（度）
STATION_TO_CAMERA_RX = 5  # 全站仪相对于相机的X轴旋转角度
STATION_TO_CAMERA_RY = 5  # 全站仪相对于相机的Y轴旋转角度
STATION_TO_CAMERA_RZ = 5  # 全站仪相对于相机的Z轴旋转角度

# 4.2 平移向量（毫米）
STATION_TO_CAMERA_TX = 2000  # 全站仪相对于相机的X方向平移
STATION_TO_CAMERA_TY = 2000  # 全站仪相对于相机的Y方向平移
STATION_TO_CAMERA_TZ = 0     # 全站仪相对于相机的Z方向平移

# 4.3 计算旋转矩阵
R_station_to_camera = Rotation.from_euler('xyz', [
    np.radians(STATION_TO_CAMERA_RX), 
    np.radians(STATION_TO_CAMERA_RY), 
    np.radians(STATION_TO_CAMERA_RZ)
]).as_matrix()

# 4.4 构建平移向量
t_station_to_camera = np.array([
    STATION_TO_CAMERA_TX, 
    STATION_TO_CAMERA_TY, 
    STATION_TO_CAMERA_TZ
])

# 4.5 构建外参矩阵 [R|t]
STATION_TO_CAMERA_EXTRINSIC = np.eye(4)
STATION_TO_CAMERA_EXTRINSIC[:3, :3] = R_station_to_camera
STATION_TO_CAMERA_EXTRINSIC[:3, 3] = t_station_to_camera

# 打印所有参数
print("="*50)
print("相机内参数")
print("="*50)
print(f"图像分辨率: {IMAGE_WIDTH}x{IMAGE_HEIGHT}像素")
print(f"焦距: {FOCAL_LENGTH_MM}mm")
print(f"像元大小: {PIXEL_SIZE_UM}微米")
print(f"焦距(像素): {FOCAL_LENGTH_PIXELS:.2f}像素")
print(f"主点坐标: ({PRINCIPAL_POINT_X}, {PRINCIPAL_POINT_Y})像素")
print("\n内参矩阵:")
print(INTRINSIC_MATRIX)

print("\n"+"="*50)
print("全站仪相对于相机的外参数")
print("="*50)
print(f"姿态角: Rx={STATION_TO_CAMERA_RX}°, Ry={STATION_TO_CAMERA_RY}°, Rz={STATION_TO_CAMERA_RZ}°")
print(f"平移向量: X={STATION_TO_CAMERA_TX}mm, Y={STATION_TO_CAMERA_TY}mm, Z={STATION_TO_CAMERA_TZ}mm")
print("\n旋转矩阵:")
print(R_station_to_camera)
print("\n外参矩阵 [R|t]:")
print(STATION_TO_CAMERA_EXTRINSIC)

# Step 5: 生成标定点
# 5.1 设置标定点参数
NUM_CALIBRATION_POINTS = 200  # 增加标定点数量
MIN_DEPTH_MM = 5000   # 5m
MAX_DEPTH_MM = 100000 # 100m

# 5.2 生成2D像素坐标（覆盖整个图像平面）
points_2d_camera_pixel = []
for _ in range(NUM_CALIBRATION_POINTS):
    # 在图像边缘区域增加采样密度
    if random.random() < 0.3:  # 30%的点在边缘区域
        u = random.choice([random.uniform(0, IMAGE_WIDTH*0.2), 
                          random.uniform(IMAGE_WIDTH*0.8, IMAGE_WIDTH)])
        v = random.choice([random.uniform(0, IMAGE_HEIGHT*0.2), 
                          random.uniform(IMAGE_HEIGHT*0.8, IMAGE_HEIGHT)])
    else:  # 70%的点在中心区域
        u = random.uniform(0, IMAGE_WIDTH)
        v = random.uniform(0, IMAGE_HEIGHT)
    points_2d_camera_pixel.append([u, v])

# 5.3 转换为相机坐标系下的3D点
points_3d_camera = []
for point_2d in points_2d_camera_pixel:
    u, v = point_2d
    # 计算归一化坐标
    x_normalized = (u - PRINCIPAL_POINT_X) / FOCAL_LENGTH_PIXELS
    y_normalized = (v - PRINCIPAL_POINT_Y) / FOCAL_LENGTH_PIXELS
    
    # 使用指数分布生成深度，使得近处点更多
    z_camera = random.expovariate(1/MAX_DEPTH_MM)
    z_camera = max(MIN_DEPTH_MM, min(MAX_DEPTH_MM, z_camera))
    
    # 计算相机坐标系下的3D坐标
    x_camera = x_normalized * z_camera
    y_camera = y_normalized * z_camera
    points_3d_camera.append([x_camera, y_camera, z_camera])

# 5.4 转换到全站仪坐标系
points_3d_station = []
R_camera_to_station = R_station_to_camera.T
t_camera_to_station = -R_camera_to_station @ t_station_to_camera
for point_3d_camera in points_3d_camera:
    # 使用从相机到全站仪的变换矩阵进行转换
    point_3d_station = R_camera_to_station @ np.array(point_3d_camera) + t_camera_to_station
    points_3d_station.append(point_3d_station)

# 打印标定点信息
print("\n"+"="*50)
print("标定点信息")
print("="*50)
print(f"标定点数量: {NUM_CALIBRATION_POINTS}")
print(f"深度范围: {MIN_DEPTH_MM/1000:.1f}m - {MAX_DEPTH_MM/1000:.1f}m")
print("\n前5个标定点的坐标:")
print("序号  相机像素坐标(u,v)    相机坐标系3D坐标(x,y,z)mm    全站仪坐标系3D坐标(x,y,z)mm")
print("-"*100)
for i in range(min(5, NUM_CALIBRATION_POINTS)):
    print(f"{i+1:2d}    ({points_2d_camera_pixel[i][0]:6.1f},{points_2d_camera_pixel[i][1]:6.1f})    "
          f"({points_3d_camera[i][0]:7.1f},{points_3d_camera[i][1]:7.1f},{points_3d_camera[i][2]:7.1f})    "
          f"({points_3d_station[i][0]:7.1f},{points_3d_station[i][1]:7.1f},{points_3d_station[i][2]:7.1f})")

# 保存标定点数据
CALIBRATION_POINTS = {
    'points_2d_camera_pixel': np.array(points_2d_camera_pixel),  # 相机像素坐标系下的2D点
    'points_3d_camera': np.array(points_3d_camera),              # 相机坐标系下的3D点
    'points_3d_station': np.array(points_3d_station)             # 全站仪坐标系下的3D点
}

# Step 6: 添加噪声并保存数据
def add_noise_and_save_to_csv():
    # 6.1 生成随机误差
    station_noise = np.random.normal(0, 1, (NUM_CALIBRATION_POINTS, 3))  # 1mm标准差
    pixel_noise = np.random.normal(0, 0.5, (NUM_CALIBRATION_POINTS, 2))  # 0.5像素标准差
    
    # 6.2 添加误差
    points_2d_with_noise = np.array(points_2d_camera_pixel) + pixel_noise
    points_3d_station_with_noise = np.array(points_3d_station) + station_noise
    
    # 6.3 创建数据字典
    data = {
        'point_id': range(1, NUM_CALIBRATION_POINTS + 1),
        'pixel_u': [p[0] for p in points_2d_camera_pixel],
        'pixel_v': [p[1] for p in points_2d_camera_pixel],
        'pixel_u_noise': pixel_noise[:, 0],
        'pixel_v_noise': pixel_noise[:, 1],
        'pixel_u_with_noise': points_2d_with_noise[:, 0],
        'pixel_v_with_noise': points_2d_with_noise[:, 1],
        'station_x': [p[0] for p in points_3d_station],
        'station_y': [p[1] for p in points_3d_station],
        'station_z': [p[2] for p in points_3d_station],
        'station_x_noise': station_noise[:, 0],
        'station_y_noise': station_noise[:, 1],
        'station_z_noise': station_noise[:, 2],
        'station_x_with_noise': points_3d_station_with_noise[:, 0],
        'station_y_with_noise': points_3d_station_with_noise[:, 1],
        'station_z_with_noise': points_3d_station_with_noise[:, 2]
    }
    
    # 6.4 创建DataFrame并保存到当前文件夹
    df = pd.DataFrame(data)
    csv_path = './calibration_points_with_noise.csv'  # 保存到当前文件夹
    df.to_csv(csv_path, index=False, float_format='%.3f')
    
    # 6.5 打印统计信息
    print("\nNoise Statistics:")
    print("Pixel Noise (std):")
    print(f"U: {np.std(pixel_noise[:, 0]):.3f} pixels")
    print(f"V: {np.std(pixel_noise[:, 1]):.3f} pixels")
    print("\nStation Noise (std):")
    print(f"X: {np.std(station_noise[:, 0]):.3f} mm")
    print(f"Y: {np.std(station_noise[:, 1]):.3f} mm")
    print(f"Z: {np.std(station_noise[:, 2]):.3f} mm")
    print(f"\nCSV file saved to: {csv_path}")
    
    return df

# Step 7: 可视化
def visualize_calibration_points():
    # 7.1 创建图形
    fig = plt.figure(figsize=(15, 7))
    
    # 7.2 绘制2D图像平面
    ax1 = fig.add_subplot(121)
    ax1.set_title('Image Plane (2D Points)')
    ax1.set_xlabel('u (pixels)')
    ax1.set_ylabel('v (pixels)')
    ax1.set_xlim(0, IMAGE_WIDTH)
    ax1.set_ylim(IMAGE_HEIGHT, 0)
    ax1.grid(True)
    
    # 7.3 绘制2D点
    points_2d = np.array(points_2d_camera_pixel)
    ax1.scatter(points_2d[:, 0], points_2d[:, 1], c='red', marker='+', label='Calibration Points')
    
    # 7.4 绘制图像边界
    ax1.plot([0, IMAGE_WIDTH, IMAGE_WIDTH, 0, 0], 
             [0, 0, IMAGE_HEIGHT, IMAGE_HEIGHT, 0], 'k-', linewidth=1)
    
    # 7.5 绘制3D相机坐标系
    ax2 = fig.add_subplot(122, projection='3d')
    ax2.set_title('Camera Coordinate System (3D Points)')
    ax2.set_xlabel('X (mm)')
    ax2.set_ylabel('Y (mm)')
    ax2.set_zlabel('Z (mm)')
    
    # 7.6 绘制3D点
    points_3d = np.array(points_3d_camera)
    ax2.scatter(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2], 
                c='blue', marker='o', label='Calibration Points')
    
    # 7.7 绘制坐标系
    origin = np.array([0, 0, 0])
    axis_length = 1000
    ax2.quiver(origin[0], origin[1], origin[2], axis_length, 0, 0, color='r', label='X')
    ax2.quiver(origin[0], origin[1], origin[2], 0, axis_length, 0, color='g', label='Y')
    ax2.quiver(origin[0], origin[1], origin[2], 0, 0, axis_length, color='b', label='Z')
    
    # 7.8 设置图形属性
    ax2.set_box_aspect([1, 1, 1])
    ax1.legend()
    ax2.legend()
    plt.tight_layout()
    plt.show()

# Step 9: 实现相机标定算法
def collinearity_equations_residuals(params, points_3d, points_2d):
    """
    计算共线方程残差（不考虑畸变）
    params: [fx, fy, cx, cy, rvec[0:3], tvec[0:3]]
    """
    # 1. 提取参数
    fx, fy, cx, cy = params[0:4]
    rvec = params[4:7]
    tvec = params[7:10]
    
    # 2. 构建相机矩阵
    camera_matrix = np.array([[fx, 0, cx],
                             [0, fy, cy],
                             [0, 0, 1]])
    
    # 3. 计算旋转矩阵
    R, _ = cv2.Rodrigues(rvec)
    
    # 4. 计算共线方程残差
    residuals = []
    for i in range(len(points_3d)):
        # 4.1 物点坐标
        X = points_3d[i]
        
        # 4.2 计算物点在相机坐标系下的坐标
        X_cam = R @ X + tvec
        
        # 4.3 计算归一化坐标
        x = X_cam[0] / X_cam[2]
        y = X_cam[1] / X_cam[2]
        
        # 4.4 计算像素坐标（不考虑畸变）
        u = fx * x + cx
        v = fy * y + cy
        
        # 4.5 计算残差
        residuals.extend([points_2d[i,0] - u, points_2d[i,1] - v])
    
    # 5. 添加旋转矩阵正交性约束（降低权重）
    ortho_error = []
    for i in range(3):
        for j in range(3):
            if i == j:
                ortho_error.append((np.dot(R[:, i], R[:, j]) - 1.0) ** 2)
            else:
                ortho_error.append(np.dot(R[:, i], R[:, j]) ** 2)
    
    # 6. 组合残差（降低正交性约束的权重）
    return np.concatenate([np.array(residuals), np.array(ortho_error)])

def analyze_residuals(params, points_3d, points_2d):
    """
    分析各个约束方程的残差（不考虑畸变）
    """
    # 1. 提取参数
    fx, fy, cx, cy = params[0:4]
    rvec = params[4:7]
    tvec = params[7:10]
    
    # 2. 计算旋转矩阵
    R, _ = cv2.Rodrigues(rvec)
    
    # 3. 分析共线方程残差
    collinearity_residuals = []
    for i in range(len(points_3d)):
        X = points_3d[i]
        X_cam = R @ X + tvec
        x = X_cam[0] / X_cam[2]
        y = X_cam[1] / X_cam[2]
        
        # 计算像素坐标（不考虑畸变）
        u = fx * x + cx
        v = fy * y + cy
        
        collinearity_residuals.append([points_2d[i,0] - u, points_2d[i,1] - v])
    
    # 4. 分析旋转矩阵正交性约束残差
    ortho_residuals = []
    for i in range(3):
        for j in range(3):
            if i == j:
                ortho_residuals.append((np.dot(R[:, i], R[:, j]) - 1.0) ** 2)
            else:
                ortho_residuals.append(np.dot(R[:, i], R[:, j]) ** 2)
    
    # 5. 计算统计信息
    collinearity_residuals = np.array(collinearity_residuals)
    ortho_residuals = np.array(ortho_residuals)
    
    return {
        'collinearity': {
            'mean': np.mean(np.abs(collinearity_residuals)),
            'std': np.std(collinearity_residuals),
            'max': np.max(np.abs(collinearity_residuals)),
            'residuals': collinearity_residuals
        },
        'orthogonality': {
            'mean': np.mean(ortho_residuals),
            'std': np.std(ortho_residuals),
            'max': np.max(ortho_residuals),
            'residuals': ortho_residuals
        }
    }

def calibrate_camera_collinearity(points_3d, points_2d, initial_K=None):
    """
    使用共线方程进行相机标定（不考虑畸变）
    """
    # 1. 初始化参数
    if initial_K is None:
        initial_K = INTRINSIC_MATRIX
    
    # 2. 使用EPNP获取初始外参
    success, rvec, tvec = cv2.solvePnP(points_3d, points_2d, initial_K, None)
    if not success:
        print("Warning: EPNP failed, using default values")
        rvec = np.zeros(3)
        tvec = np.zeros(3)
    
    # 2.1 验证初始值
    R_init, _ = cv2.Rodrigues(rvec)
    if not np.allclose(R_init @ R_init.T, np.eye(3), atol=1e-3):
        print("Warning: Initial rotation matrix is not orthogonal")
        # 使用SVD强制正交化
        U, _, Vt = np.linalg.svd(R_init)
        R_init = U @ Vt
        rvec, _ = cv2.Rodrigues(R_init)
    
    # 3. 设置初始参数（移除畸变参数）
    initial_params = np.concatenate([
        [initial_K[0,0], initial_K[1,1], initial_K[0,2], initial_K[1,2]],  # fx, fy, cx, cy
        rvec.ravel(),  # 旋转向量
        tvec.ravel()   # 平移向量
    ])
    
    # 4. 执行优化（调整优化参数）
    result = least_squares(
        collinearity_equations_residuals,
        initial_params,
        args=(points_3d, points_2d),
        method='lm',
        max_nfev=1000,        # 增加最大迭代次数
        ftol=1e-15,          # 放宽函数值收敛条件
        xtol=1e-15,          # 放宽参数收敛条件
        verbose=1
    )
    
    # 5. 提取优化结果
    optimized_params = result.x
    fx, fy, cx, cy = optimized_params[0:4]
    rvec = optimized_params[4:7]
    tvec = optimized_params[7:10]
    
    # 6. 构建最终相机矩阵
    camera_matrix = np.array([[fx, 0, cx],
                             [0, fy, cy],
                             [0, 0, 1]])
    
    # 7. 计算共线方程残差
    residuals = collinearity_equations_residuals(optimized_params, points_3d, points_2d)
    mean_error = np.mean(np.abs(residuals[:len(points_3d)*2]))
    
    return {
        'camera_matrix': camera_matrix,
        'rvec': rvec,
        'tvec': tvec,
        'mean_error': mean_error
    }

class CalibrationGUI:
    def __init__(self, root):
        self.root = root
        self.root.title("相机标定程序")
        
        # 创建主框架
        main_frame = ttk.Frame(root, padding="10")
        main_frame.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 参数设置区域
        param_frame = ttk.LabelFrame(main_frame, text="参数设置", padding="5")
        param_frame.grid(row=0, column=0, padx=5, pady=5, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 相机参数
        ttk.Label(param_frame, text="相机参数:").grid(row=0, column=0, sticky=tk.W)
        
        # 图像分辨率
        ttk.Label(param_frame, text="图像宽度:").grid(row=1, column=0, sticky=tk.W)
        self.image_width = ttk.Entry(param_frame, width=10)
        self.image_width.insert(0, str(IMAGE_WIDTH))
        self.image_width.grid(row=1, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="图像高度:").grid(row=2, column=0, sticky=tk.W)
        self.image_height = ttk.Entry(param_frame, width=10)
        self.image_height.insert(0, str(IMAGE_HEIGHT))
        self.image_height.grid(row=2, column=1, padx=5, pady=2)
        
        # 物理参数
        ttk.Label(param_frame, text="焦距(mm):").grid(row=3, column=0, sticky=tk.W)
        self.focal_length = ttk.Entry(param_frame, width=10)
        self.focal_length.insert(0, str(FOCAL_LENGTH_MM))
        self.focal_length.grid(row=3, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="像元大小(μm):").grid(row=4, column=0, sticky=tk.W)
        self.pixel_size = ttk.Entry(param_frame, width=10)
        self.pixel_size.insert(0, str(PIXEL_SIZE_UM))
        self.pixel_size.grid(row=4, column=1, padx=5, pady=2)
        
        # 标定点参数
        ttk.Label(param_frame, text="标定点参数:").grid(row=5, column=0, sticky=tk.W)
        
        ttk.Label(param_frame, text="标定点数量:").grid(row=6, column=0, sticky=tk.W)
        self.num_points = ttk.Entry(param_frame, width=10)
        self.num_points.insert(0, str(NUM_CALIBRATION_POINTS))
        self.num_points.grid(row=6, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="最小深度(m):").grid(row=7, column=0, sticky=tk.W)
        self.min_depth = ttk.Entry(param_frame, width=10)
        self.min_depth.insert(0, str(MIN_DEPTH_MM/1000))
        self.min_depth.grid(row=7, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="最大深度(m):").grid(row=8, column=0, sticky=tk.W)
        self.max_depth = ttk.Entry(param_frame, width=10)
        self.max_depth.insert(0, str(MAX_DEPTH_MM/1000))
        self.max_depth.grid(row=8, column=1, padx=5, pady=2)
        
        # 噪声参数
        ttk.Label(param_frame, text="噪声参数:").grid(row=9, column=0, sticky=tk.W)
        
        ttk.Label(param_frame, text="像素噪声(std):").grid(row=10, column=0, sticky=tk.W)
        self.pixel_noise = ttk.Entry(param_frame, width=10)
        self.pixel_noise.insert(0, "0.5")
        self.pixel_noise.grid(row=10, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="全站仪噪声(std):").grid(row=11, column=0, sticky=tk.W)
        self.station_noise = ttk.Entry(param_frame, width=10)
        self.station_noise.insert(0, "1.0")
        self.station_noise.grid(row=11, column=1, padx=5, pady=2)
        
        # 在参数设置区域添加初始误差设置
        ttk.Label(param_frame, text="初始误差设置:").grid(row=13, column=0, sticky=tk.W)
        
        # 姿态角初始误差
        ttk.Label(param_frame, text="Rx初始误差(°):").grid(row=14, column=0, sticky=tk.W)
        self.rx_error = ttk.Entry(param_frame, width=10)
        self.rx_error.insert(0, "0")
        self.rx_error.grid(row=14, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="Ry初始误差(°):").grid(row=15, column=0, sticky=tk.W)
        self.ry_error = ttk.Entry(param_frame, width=10)
        self.ry_error.insert(0, "0")
        self.ry_error.grid(row=15, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="Rz初始误差(°):").grid(row=16, column=0, sticky=tk.W)
        self.rz_error = ttk.Entry(param_frame, width=10)
        self.rz_error.insert(0, "0")
        self.rz_error.grid(row=16, column=1, padx=5, pady=2)
        
        # 平移向量初始误差
        ttk.Label(param_frame, text="X初始误差(mm):").grid(row=17, column=0, sticky=tk.W)
        self.tx_error = ttk.Entry(param_frame, width=10)
        self.tx_error.insert(0, "0")
        self.tx_error.grid(row=17, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="Y初始误差(mm):").grid(row=18, column=0, sticky=tk.W)
        self.ty_error = ttk.Entry(param_frame, width=10)
        self.ty_error.insert(0, "0")
        self.ty_error.grid(row=18, column=1, padx=5, pady=2)
        
        ttk.Label(param_frame, text="Z初始误差(mm):").grid(row=19, column=0, sticky=tk.W)
        self.tz_error = ttk.Entry(param_frame, width=10)
        self.tz_error.insert(0, "0")
        self.tz_error.grid(row=19, column=1, padx=5, pady=2)
        
        # 开始按钮
        self.start_button = ttk.Button(param_frame, text="开始标定", command=self.start_calibration)
        self.start_button.grid(row=20, column=0, columnspan=2, pady=10)
        
        # 结果显示区域
        result_frame = ttk.LabelFrame(main_frame, text="标定结果", padding="5")
        result_frame.grid(row=0, column=1, padx=5, pady=5, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 创建两个子框架：参数结果和误差分析
        param_result_frame = ttk.LabelFrame(result_frame, text="参数结果", padding="5")
        param_result_frame.grid(row=0, column=0, padx=5, pady=5, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        error_result_frame = ttk.LabelFrame(result_frame, text="误差分析", padding="5")
        error_result_frame.grid(row=1, column=0, padx=5, pady=5, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 参数结果显示
        self.param_text = scrolledtext.ScrolledText(param_result_frame, width=60, height=15)
        self.param_text.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 误差结果显示
        self.error_text = scrolledtext.ScrolledText(error_result_frame, width=60, height=15)
        self.error_text.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 添加可视化框架
        viz_frame = ttk.LabelFrame(main_frame, text="可视化结果", padding="5")
        viz_frame.grid(row=1, column=0, columnspan=2, padx=5, pady=5, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 创建matplotlib画布
        self.fig = plt.Figure(figsize=(10, 4))
        self.canvas = FigureCanvasTkAgg(self.fig, master=viz_frame)
        self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
        
        # 创建子图
        self.ax1 = self.fig.add_subplot(121)
        self.ax2 = self.fig.add_subplot(122, projection='3d')
        
        # 设置网格权重
        main_frame.columnconfigure(1, weight=1)
        main_frame.rowconfigure(0, weight=1)
        result_frame.columnconfigure(0, weight=1)
        result_frame.rowconfigure(0, weight=1)
        result_frame.rowconfigure(1, weight=1)
        param_result_frame.columnconfigure(0, weight=1)
        param_result_frame.rowconfigure(0, weight=1)
        error_result_frame.columnconfigure(0, weight=1)
        error_result_frame.rowconfigure(0, weight=1)
        viz_frame.columnconfigure(0, weight=1)
        viz_frame.rowconfigure(0, weight=1)
    
    def generate_calibration_points(self, num_points, image_width, image_height, min_depth, max_depth):
        """生成标定点"""
        points_2d_camera_pixel = []
        points_3d_camera = []
        
        for _ in range(num_points):
            # 在图像边缘区域增加采样密度
            if random.random() < 0.3:  # 30%的点在边缘区域
                u = random.choice([random.uniform(0, image_width*0.2), 
                                 random.uniform(image_width*0.8, image_width)])
                v = random.choice([random.uniform(0, image_height*0.2), 
                                 random.uniform(image_height*0.8, image_height)])
            else:  # 70%的点在中心区域
                u = random.uniform(0, image_width)
                v = random.uniform(0, image_height)
            
            # 计算归一化坐标
            x_normalized = (u - image_width/2) / (FOCAL_LENGTH_MM * 1000 / PIXEL_SIZE_UM)
            y_normalized = (v - image_height/2) / (FOCAL_LENGTH_MM * 1000 / PIXEL_SIZE_UM)
            
            # 使用指数分布生成深度，使得近处点更多
            z_camera = random.expovariate(1/max_depth)
            z_camera = max(min_depth, min(max_depth, z_camera))
            
            # 计算相机坐标系下的3D坐标
            x_camera = x_normalized * z_camera
            y_camera = y_normalized * z_camera
            
            points_2d_camera_pixel.append([u, v])
            points_3d_camera.append([x_camera, y_camera, z_camera])
        
        return np.array(points_2d_camera_pixel), np.array(points_3d_camera)
    
    def start_calibration(self):
        # 禁用开始按钮
        self.start_button.state(['disabled'])
        
        # 在新线程中运行标定
        thread = threading.Thread(target=self.run_calibration)
        thread.start()
    
    def run_calibration(self):
        try:
            # 获取参数
            global IMAGE_WIDTH, IMAGE_HEIGHT, FOCAL_LENGTH_MM, PIXEL_SIZE_UM
            global NUM_CALIBRATION_POINTS, MIN_DEPTH_MM, MAX_DEPTH_MM
            
            # 从界面获取最新参数
            IMAGE_WIDTH = int(self.image_width.get())
            IMAGE_HEIGHT = int(self.image_height.get())
            FOCAL_LENGTH_MM = float(self.focal_length.get())
            PIXEL_SIZE_UM = float(self.pixel_size.get())
            NUM_CALIBRATION_POINTS = int(self.num_points.get())
            MIN_DEPTH_MM = float(self.min_depth.get()) * 1000
            MAX_DEPTH_MM = float(self.max_depth.get()) * 1000
            
            # 更新显示
            self.param_text.delete(1.0, tk.END)
            self.error_text.delete(1.0, tk.END)
            self.param_text.insert(tk.END, "开始标定...\n\n")
            self.root.update()
            
            # 使用最新参数重新生成标定点
            points_2d, points_3d_camera = self.generate_calibration_points(
                NUM_CALIBRATION_POINTS, 
                IMAGE_WIDTH, 
                IMAGE_HEIGHT, 
                MIN_DEPTH_MM, 
                MAX_DEPTH_MM
            )
            
            # 转换到全站仪坐标系
            points_3d_station = []
            R_camera_to_station = R_station_to_camera.T
            t_camera_to_station = -R_camera_to_station @ t_station_to_camera
            for point_3d_camera in points_3d_camera:
                point_3d_station = R_camera_to_station @ point_3d_camera + t_camera_to_station
                points_3d_station.append(point_3d_station)
            points_3d_station = np.array(points_3d_station)
            
            # 添加噪声
            pixel_noise = np.random.normal(0, float(self.pixel_noise.get()), points_2d.shape)
            station_noise = np.random.normal(0, float(self.station_noise.get()), points_3d_station.shape)
            
            points_2d_noisy = points_2d + pixel_noise
            points_3d_noisy = points_3d_station + station_noise
            
            # 更新内参矩阵
            FOCAL_LENGTH_PIXELS = FOCAL_LENGTH_MM * 1000 / PIXEL_SIZE_UM
            PRINCIPAL_POINT_X = IMAGE_WIDTH / 2
            PRINCIPAL_POINT_Y = IMAGE_HEIGHT / 2
            INTRINSIC_MATRIX = np.array([
                [FOCAL_LENGTH_PIXELS, 0, PRINCIPAL_POINT_X],
                [0, FOCAL_LENGTH_PIXELS, PRINCIPAL_POINT_Y],
                [0, 0, 1]
            ])
            
            # 获取初始误差值
            rx_error = float(self.rx_error.get())
            ry_error = float(self.ry_error.get())
            rz_error = float(self.rz_error.get())
            tx_error = float(self.tx_error.get())
            ty_error = float(self.ty_error.get())
            tz_error = float(self.tz_error.get())
            
            # 计算带误差的初始旋转矩阵
            R_error = Rotation.from_euler('xyz', [
                np.radians(rx_error),
                np.radians(ry_error),
                np.radians(rz_error)
            ]).as_matrix()
            
            # 计算带误差的初始平移向量
            t_error = np.array([tx_error, ty_error, tz_error])
            
            # 获取初始值
            initial_params = np.concatenate([
                [INTRINSIC_MATRIX[0,0], INTRINSIC_MATRIX[1,1], 
                 INTRINSIC_MATRIX[0,2], INTRINSIC_MATRIX[1,2]],
                cv2.Rodrigues(R_error)[0].ravel(),  # 带误差的初始旋转向量
                t_error  # 带误差的初始平移向量
            ])
            
            # 在结果显示中添加初始误差信息
            self.param_text.insert(tk.END, "初始误差设置:\n")
            self.param_text.insert(tk.END, f"姿态角误差: Rx={rx_error}°, Ry={ry_error}°, Rz={rz_error}°\n")
            self.param_text.insert(tk.END, f"平移向量误差: X={tx_error}mm, Y={ty_error}mm, Z={tz_error}mm\n\n")
            
            # 执行标定
            self.param_text.insert(tk.END, "执行标定...\n")
            self.root.update()
            
            # 计算初始残差
            initial_residuals = collinearity_equations_residuals(
                initial_params, points_3d_noisy, points_2d_noisy
            )
            initial_error = np.mean(np.abs(initial_residuals[:len(points_3d_noisy)*2]))
            
            # 执行标定
            calibration_result = calibrate_camera_collinearity(points_3d_noisy, points_2d_noisy, INTRINSIC_MATRIX)
            
            # 分析残差
            residuals_analysis = analyze_residuals(
                np.concatenate([
                    [calibration_result['camera_matrix'][0,0], 
                     calibration_result['camera_matrix'][1,1],
                     calibration_result['camera_matrix'][0,2],
                     calibration_result['camera_matrix'][1,2]],
                    calibration_result['rvec'],
                    calibration_result['tvec']
                ]),
                points_3d_noisy,
                points_2d_noisy
            )
            
            # 显示参数结果
            self.param_text.insert(tk.END, "标定结果:\n")
            self.param_text.insert(tk.END, f"相机矩阵:\n{calibration_result['camera_matrix']}\n\n")
            self.param_text.insert(tk.END, f"旋转向量:\n{calibration_result['rvec']}\n\n")
            self.param_text.insert(tk.END, f"平移向量:\n{calibration_result['tvec']}\n\n")
            self.param_text.insert(tk.END, f"平均误差: {calibration_result['mean_error']:.6f} 像素\n\n")
            
            # 显示初值误差
            self.error_text.insert(tk.END, "初值误差分析:\n")
            self.error_text.insert(tk.END, f"初始平均误差: {initial_error:.6f} 像素\n")
            self.error_text.insert(tk.END, f"优化后平均误差: {calibration_result['mean_error']:.6f} 像素\n")
            self.error_text.insert(tk.END, f"误差改善: {(initial_error - calibration_result['mean_error']):.6f} 像素\n\n")
            
            # 在误差分析中添加初始误差的影响
            self.error_text.insert(tk.END, "初始误差影响分析:\n")
            self.error_text.insert(tk.END, f"初始姿态角误差: {np.linalg.norm(R_error - np.eye(3)):.6f}\n")
            self.error_text.insert(tk.END, f"初始平移向量误差: {np.linalg.norm(t_error):.6f} mm\n\n")
            
            # 计算参数误差
            # 内参误差
            fx_error = abs(calibration_result['camera_matrix'][0,0] - FOCAL_LENGTH_PIXELS)
            fy_error = abs(calibration_result['camera_matrix'][1,1] - FOCAL_LENGTH_PIXELS)
            cx_error = abs(calibration_result['camera_matrix'][0,2] - PRINCIPAL_POINT_X)
            cy_error = abs(calibration_result['camera_matrix'][1,2] - PRINCIPAL_POINT_Y)
            
            # 外参误差
            R_calibrated, _ = cv2.Rodrigues(calibration_result['rvec'])
            R_error = np.linalg.norm(R_calibrated - R_station_to_camera)
            t_error = np.linalg.norm(calibration_result['tvec'] - t_station_to_camera)
            
            # 计算姿态角误差
            calibrated_euler = Rotation.from_matrix(R_calibrated).as_euler('xyz', degrees=True)
            true_euler = Rotation.from_matrix(R_station_to_camera).as_euler('xyz', degrees=True)
            rx_error = abs(calibrated_euler[0] - true_euler[0])
            ry_error = abs(calibrated_euler[1] - true_euler[1])
            rz_error = abs(calibrated_euler[2] - true_euler[2])
            
            # 显示误差分析
            self.error_text.insert(tk.END, "参数误差分析:\n\n")
            
            # 内参误差
            self.error_text.insert(tk.END, "内参误差:\n")
            self.error_text.insert(tk.END, f"fx误差: {fx_error:.6f} 像素\n")
            self.error_text.insert(tk.END, f"fy误差: {fy_error:.6f} 像素\n")
            self.error_text.insert(tk.END, f"cx误差: {cx_error:.6f} 像素\n")
            self.error_text.insert(tk.END, f"cy误差: {cy_error:.6f} 像素\n\n")
            
            # 外参误差
            self.error_text.insert(tk.END, "外参误差:\n")
            self.error_text.insert(tk.END, f"旋转矩阵误差: {R_error:.6f}\n")
            self.error_text.insert(tk.END, f"平移向量误差: {t_error:.6f} mm\n\n")
            
            # 姿态角误差
            self.error_text.insert(tk.END, "姿态角误差 (度):\n")
            self.error_text.insert(tk.END, f"Rx误差: {rx_error:.6f}°\n")
            self.error_text.insert(tk.END, f"Ry误差: {ry_error:.6f}°\n")
            self.error_text.insert(tk.END, f"Rz误差: {rz_error:.6f}°\n\n")
            
            # 残差分析
            self.error_text.insert(tk.END, "残差分析:\n")
            self.error_text.insert(tk.END, f"共线方程残差 (像素):\n")
            self.error_text.insert(tk.END, f"平均值: {residuals_analysis['collinearity']['mean']:.6f}\n")
            self.error_text.insert(tk.END, f"标准差: {residuals_analysis['collinearity']['std']:.6f}\n")
            self.error_text.insert(tk.END, f"最大值: {residuals_analysis['collinearity']['max']:.6f}\n\n")
            
            self.error_text.insert(tk.END, f"旋转矩阵正交性约束:\n")
            self.error_text.insert(tk.END, f"平均值: {residuals_analysis['orthogonality']['mean']:.6f}\n")
            self.error_text.insert(tk.END, f"标准差: {residuals_analysis['orthogonality']['std']:.6f}\n")
            self.error_text.insert(tk.END, f"最大值: {residuals_analysis['orthogonality']['max']:.6f}\n")
            
            # 可视化结果
            self.visualize_results(points_2d, points_3d_camera, points_2d_noisy, points_3d_noisy)
            
        except Exception as e:
            self.param_text.insert(tk.END, f"\n错误: {str(e)}\n")
            self.error_text.insert(tk.END, f"\n错误: {str(e)}\n")
        finally:
            self.start_button.state(['!disabled'])

    def visualize_results(self, points_2d, points_3d_camera, points_2d_noisy, points_3d_noisy):
        """Visualize calibration results"""
        # 清除旧图
        self.ax1.clear()
        self.ax2.clear()
        
        # 绘制2D图像平面
        self.ax1.set_title('Image Plane (2D Points)')
        self.ax1.set_xlabel('u (pixels)')
        self.ax1.set_ylabel('v (pixels)')
        self.ax1.set_xlim(0, IMAGE_WIDTH)
        self.ax1.set_ylim(IMAGE_HEIGHT, 0)
        self.ax1.grid(True)
        
        # 绘制原始点和带噪声的点
        self.ax1.scatter(points_2d[:, 0], points_2d[:, 1], c='blue', marker='+', label='Original Points')
        self.ax1.scatter(points_2d_noisy[:, 0], points_2d_noisy[:, 1], c='red', marker='.', label='Noisy Points')
        
        # 绘制图像边界
        self.ax1.plot([0, IMAGE_WIDTH, IMAGE_WIDTH, 0, 0], 
                     [0, 0, IMAGE_HEIGHT, IMAGE_HEIGHT, 0], 'k-', linewidth=1)
        
        # 绘制3D点
        self.ax2.set_title('Camera Coordinate System (3D Points)')
        self.ax2.set_xlabel('X (mm)')
        self.ax2.set_ylabel('Y (mm)')
        self.ax2.set_zlabel('Z (mm)')
        
        # 绘制原始点和带噪声的点
        self.ax2.scatter(points_3d_camera[:, 0], points_3d_camera[:, 1], points_3d_camera[:, 2], 
                        c='blue', marker='o', label='Original Points')
        self.ax2.scatter(points_3d_noisy[:, 0], points_3d_noisy[:, 1], points_3d_noisy[:, 2], 
                        c='red', marker='.', label='Noisy Points')
        
        # 绘制坐标系
        origin = np.array([0, 0, 0])
        axis_length = 1000
        self.ax2.quiver(origin[0], origin[1], origin[2], axis_length, 0, 0, color='r', label='X')
        self.ax2.quiver(origin[0], origin[1], origin[2], 0, axis_length, 0, color='g', label='Y')
        self.ax2.quiver(origin[0], origin[1], origin[2], 0, 0, axis_length, color='b', label='Z')
        
        # 设置图形属性
        self.ax2.set_box_aspect([1, 1, 1])
        self.ax1.legend()
        self.ax2.legend()
        
        # 更新画布
        self.fig.tight_layout()
        self.canvas.draw()

if __name__ == "__main__":
    root = tk.Tk()
    app = CalibrationGUI(root)
    root.mainloop()
